/**
 * @file unitree_motor_control.h
 * @brief 
 * @version 0.1
 * @date 2025-03-03
 * @attention 因为GO电机和A1电机的协议不同，所以会有部分通用部分专用，但因为顺序问题比较混乱，
 *            命名为GO/A1...用以区分
 *            但是其实在使用的时候只有接收消息的部分需要区分
 *            同时如果不需要重新开发的话，直接使用unitree_motor_control.c里面的两个HAL_StatusTypeDef类型的函数即可
 *            注意串口波特率的问题，GO电机使用波特率为4.0M，A1电机使用波特率为4.8M，如果不能直接设置的话尽量找到一个
 *            误差较小的波特率，实测A1电机的波特率误差在±2%以内可以使用，也就是4.7M~4.9M
 */

#ifndef __UNITREE_MOTOR_CONTORL_H 
#define __UNITREE_MOTOR_CONTORL_H 
 
#include <stdint.h> 
#include "main.h" // stm32 hal 
#include "struct_typedef.h"
#include "usart.h"
#include "remote_control.h"
// #include "crc.h"   //crc已融合进驱动电机的文件中

#define SATURATE(_IN, _MIN, _MAX) {\
 if (_IN < _MIN)\
 _IN = _MIN;\
 else if (_IN > _MAX)\
 _IN = _MAX;\
} 
#define SIGN(x) ((x) > 0 ? 1 : ((x) < 0 ? -1 : 0))

/*区分电机类型结构体*/
typedef enum
{
	A1_motor,
	GO_motor,
}motor_type; 

#pragma pack(1) // 1Byte对齐!不可删除! 
/*GO电机模式控制信息*/
typedef struct
{
    uint8_t id     :4;      // 电机ID: 0,1...,13,14 15表示向所有电机广播数据(此时无返回)
    uint8_t status :3;      // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
    uint8_t none   :1;      // 保留位
} GO_Mode_t;   // 控制模式 1Byte

/*GO电机状态控制信息*/
typedef struct
{
    int16_t tor_des;        // 期望关节输出扭矩 unit: N.m      (q8)
    int16_t spd_des;        // 期望关节输出速度 unit: rad/s    (q8)
    int32_t pos_des;        // 期望关节输出位置 unit: rad      (q15)
    int16_t k_pos;          // 期望关节刚度系数 unit: -1.0-1.0 (q15)
    int16_t k_spd;          // 期望关节阻尼系数 unit: -1.0-1.0 (q15)
    
} GO_Comd_t;   // 控制参数 12Byte

/*GO电机状态反馈信息*/
typedef struct
{
    int16_t  torque;        // 实际关节输出扭矩 unit: N.m     (q8)
    int16_t  speed;         // 实际关节输出速度 unit: rad/s   (q8)
    int32_t  pos;           // 实际关节输出位置 unit: rad     (q15)
    int8_t   temp;          // 电机温度: -128~127°C
    uint8_t  MError :3;     // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留
    uint16_t force  :12;    // 足端气压传感器数据 12bit (0-4095)
    uint8_t  none   :1;     // 保留位
} GO_Fbk_t;   // 状态数据 11Byte

/*A1电机模式控制信息*/
typedef union 
{ 
    int32_t Len; 
    uint8_t u8[4]; 
    uint16_t u16[2]; 
    uint32_t u32; 
    float F; 
} A1_COM_Data32; 


typedef struct 
{ 
    // 定义 数据包头 
    unsigned char start[2]; // 包头 start[0] = 0xFE start[1] = 0xEE 
    unsigned char motorID;  // 电机ID  0,1,2,3 ...   0xBB 表示向所有电机广播（此时无返回） 
    unsigned char reserved; 
} A1_COM_Head; 
 
typedef struct 
{ // 以 4个字节一组排列 ，不然编译器会凑整 
    // 定义 数据 
    uint8_t mode;    // 当前关节模式 
    uint8_t ReadBit; // 电机控制参数修改     是否成功位 
    int8_t Temp;     // 电机当前平均温度 
    uint8_t MError;  // 电机错误 标识 
 
    A1_COM_Data32 Read; // 读取的当前 电机 的控制数据 
    int16_t T;      // 当前实际电机输出力矩       7 + 8 描述 
 
    int16_t Speed; // 当前实际电机速度（高速）   8 + 7 描述 
    float LW;  // 当前实际电机速度（低速） 
 
    int16_t W2; // 当前实际关节速度（高速）   8 + 7 描述 
    float LW2;  // 当前实际关节速度（低速） 
 
    int16_t Acc;    // 电机转子加速度       15+0 描述  惯量较小 
    int16_t OutAcc; // 输出轴加速度         12+3 描述  惯量较大 
 
    int32_t Pos;  // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） 
    int32_t Pos2; // 关节编码器位置(输出编码器) 
 
    int16_t gyro[3]; // 电机驱动板6轴传感器数据 
    int16_t acc[3]; 
 
    // 力传感器的数据 
    int16_t Fgyro[3]; 
    int16_t Facc[3]; 
    int16_t Fmag[3]; 
    uint8_t Ftemp; // 8位表示的温度  7位（-28~100度）  1位0.5度分辨率 
 
    int16_t Force16; // 力传感器高16位数据 
    int8_t Force8;   // 力传感器低8位数据 
 
    uint8_t FError; //  足端传感器错误标识 
 
    int8_t Res[1]; // 通讯 保留字节 
 
} A1_ComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4） 

/*A1电机控制命令数据包*/
typedef struct 
{ 
    A1_COM_Head head; 
    A1_ComdV3 Mdata; 
    A1_COM_Data32 CRCdata; 
 
} A1_ComdDataV3;  

/*GO电机控制命令数据包*/
typedef struct
{
    // 定义 电机控制命令数据包
    uint8_t head[2];    // 包头         2Byte
    GO_Mode_t mode;    // 电机控制模式  1Byte
    GO_Comd_t comd;    // 电机期望数据 12Byte
    uint16_t   CRC16;   // CRC          2Byte
} GO_Ctrl_Data_t;     //电机控制命令数据包

/*通用结构体*/
typedef struct 
{ 
    uint8_t none[8]; // 保留 
 
} LowHzMotorCmd; 
 
typedef struct 
{                      // 以 4个字节一组排列 ，不然编译器会凑整 
                       // 定义 数据 
    uint8_t mode;      // 关节模式选择 
    uint8_t ModifyBit; // 电机控制参数修改位 
    uint8_t ReadBit;   // 电机控制参数发送位 
    uint8_t reserved; 
 
    A1_COM_Data32 Modify; // 电机参数修改 的数据 
    // 实际给FOC的指令力矩为： 
    //  K_P*delta_Pos + K_W*delta_W + T 
    int16_t T;   // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述 
    int16_t Speed;   // 期望关节速度 （电机本身的速度） x128,       8 + 7描述 
    int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准） 
 
    int16_t K_P; // 关节刚度系数 x78.5577 倍描述 
    int16_t K_W; // 关节速度系数 x102400  倍描述 
 
    uint8_t LowHzMotorCmdIndex; // 保留 
    uint8_t LowHzMotorCmdByte;  // 保留 
 
    A1_COM_Data32 Res[1]; // 通讯 保留字节  用于实现别的一些通讯内容 
 
} MasterComdV3; // 加上数据包的包头 和CRC 34字节 
/*A1电机返回数据*/ 
typedef struct 
{ 
    // 定义 电机控制命令数据包 
    A1_COM_Head head; 
    MasterComdV3 Mdata; 
    A1_COM_Data32 CRCdata; 
} A1_MotorData_t; // 返回数据 
 
/*GO电机返回数据*/
typedef struct
{
    uint8_t head[2];    // 包头         2Byte
    GO_Mode_t mode;    // 电机控制模式  1Byte
    GO_Fbk_t   fbk;    // 电机反馈数据 11Byte
    uint16_t  CRC16;    // CRC          2Byte
} GO_MotorData_t; 

#pragma pack() // 结束 1Byte对齐!不可删除! 
 
typedef struct 
{
    float footForce;	// 足端气压传感器数据 12bit (0-4095)
}go_motor_recv;

typedef struct 
{
    float LW;  // 当前实际电机速度（低速） 
    int Acc;   // 电机转子加速度 
    float gyro[3]; // 电机驱动板6轴传感器数据 
    float acc[3]; 
}a1_motor_recv;

/*宇树电机接收数据结构体*/
typedef struct 
{ 
    // 定义 发送格式化数据 
    GO_Ctrl_Data_t go_send_data; // 电机控制数据结构体 
    A1_MotorData_t a1_send_data; // 电机控制数据结构体 
    int hex_len;                      // 发送的16进制命令数组长度, 34 
    long long send_time;              // 发送该命令的时间, 微秒(us) 
    // 待发送的各项数据 
    unsigned short id;   // 电机ID，0代表全部电机 
    unsigned short mode; // 0:空闲, 5:开环转动, 10:闭环FOC控制 
    // 实际给FOC的指令力矩为： 
    //  K_P*delta_Pos + K_W*delta_W + T 
    float T;       // 期望关节的输出力矩（电机本身的力矩）（Nm） 
    float Speed;       // 期望关节速度（电机本身的速度）(rad/s) 
    float Pos;     // 期望关节位置（rad） 
    float K_P;     // 关节刚度系数 
    float K_W;     // 关节速度系数 
    GO_Comd_t     GO_Res; // 通讯 保留字节  用于实现别的一些通讯内容 
    A1_COM_Data32 A1_Res; // 通讯 保留字节  用于实现别的一些通讯内容 
} UNITREE_MOTOR_send; 

typedef struct
{
    // 定义 接收数据
    GO_MotorData_t go_recv_data;    //电机接收数据结构体，详见motor_msg.h
    A1_ComdDataV3  a1_recv_data;     // 电机接收数据结构体，详见motor_msg.h 
    int hex_len;                        //接收的16进制命令数组长度, 78
    long long resv_time;                //接收该命令的时间, 微秒(us)
    int correct;                        //接收数据是否完整（1完整，0不完整）
    //解读得出的电机数据
    unsigned char motor_id;             //电机ID
    unsigned char mode;                 // 0:空闲, 5:开环转动, 10:闭环FOC控制
    int Temp;                           //温度
    unsigned char MError;               //错误码
    float T;                            // 当前实际电机输出力矩
	float Speed;						    // speed
    float Pos;                          // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
	a1_motor_recv a1_motor_r;
	go_motor_recv go_motor_r;

} UNITREE_MOTOR_recv;

typedef struct {
    float target_speed;   // 目标速度（来自遥控器原始输入）
    float current_speed;   // 当前实际输出速度
    float acceleration;    // 最大加速度（rad/s?）
    uint32_t last_time;    // 上次更新时间戳
} SpeedRampController;


extern UNITREE_MOTOR_send unitree_motor_s ; //发送数据结构体
extern UNITREE_MOTOR_recv unitree_motor_r ; //接受数据结构体

#define SET_485_DE_UP()     HAL_GPIO_WritePin(GPIOF, GPIO_PIN_1, GPIO_PIN_SET)
#define SET_485_DE_DOWN()   HAL_GPIO_WritePin(GPIOF, GPIO_PIN_1, GPIO_PIN_RESET)

#define SET_485_RE_UP()     HAL_GPIO_WritePin(GPIOF, GPIO_PIN_0, GPIO_PIN_SET)
#define SET_485_RE_DOWN()   HAL_GPIO_WritePin(GPIOF, GPIO_PIN_0, GPIO_PIN_RESET)

#define unitree_huart huart1
#define pitch_motor_id 0

#define go_empty_kp 0.5
#define go_empty_kw 0.02
#define go_stop  0
#define go_start 1
#define a1_stop  0
#define a1_start 10

extern float Init_Pos;
extern float Min_Pos;
//#define pitch_min_pos Init_Pos
//#define pitch_max_pos 1.89f
#define pitch_range 0.8f					//pitch行程
#define pitch_max_speed 20.0f
#define DECEL_DISTANCE 10.0f      // 开始减速的距离（根据最大速度和减速度调整）
#define EMERGENCY_MARGIN 2.0f     // 硬限位触发的安全余量
#define Pitch_KW 0.05
#define Pitch_KP 5

// uint32_t A1_crc_core(uint32_t *ptr, uint32_t len); 
int A1_extract_data (UNITREE_MOTOR_recv *motor_r) ;
int A1_modify_data  (UNITREE_MOTOR_send *motor_s) ;
int GO_modify_data  (UNITREE_MOTOR_send *motor_s);
int GO_extract_data (UNITREE_MOTOR_recv *motor_r);
HAL_StatusTypeDef unitree_uart_Send_recv(motor_type motor ,UART_HandleTypeDef *huart,UNITREE_MOTOR_send *pData, UNITREE_MOTOR_recv *rData);
HAL_StatusTypeDef unitree_rc_ctrl(motor_type motor_type, UART_HandleTypeDef *huart, UNITREE_MOTOR_send *p_data, UNITREE_MOTOR_recv *r_data,remoter_t *rc);
float speed_limit(float raw_speed,float last_pos);
/**********crc校验**********/
//go电机crc校验，采用查表法
static uint16_t const GO_crc_core_table[256] = {
	0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
	0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
	0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
	0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
	0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
	0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
	0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
	0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
	0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
	0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};

static uint16_t GO_crc_core_byte(uint16_t crc, const uint8_t c)
{
	return (crc >> 8) ^ GO_crc_core_table[(crc ^ c) & 0xff];
}

static uint16_t GO_crc_core(uint16_t crc, uint8_t const *buffer, size_t len)
{
	uint16_t tmp = crc;
	while (len--)
		tmp = GO_crc_core_byte(tmp, *buffer++);
	return tmp;
}

uint32_t A1_crc_core(uint32_t *ptr, uint32_t len) ;

#endif 

